/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include "main.h"
#include "acb_parm.h"
#include "calc_cor_eng_adapt.h"
#include "calc_adapt_conv.h"
#include "end_correct_adapt.h"
#include "replicate_short_to_full_calc_cor_eng_adapt.h"
#include "move_array16.h"
#include <assert.h>


#if 0  /* routine now implemented in its own file */
STATIC void CalcAdaptConv(
fxpt_16 pc_imp[],
int     len_trunc,
fxpt_16 Exc[],
int     Exc_len,
fxpt_16 conv[]);
#endif

#if 0 /* now implemented in its own file */
STATIC void EndCorrectAdapt(
fxpt_16 conv[],
fxpt_16 Exc[],
int     Exc_len,
fxpt_16 pc_imp[],
int     len_trunc);
#endif

#if 0  /* Routine no longer called */
STATIC void ReplicateShortToFull(
int     delay,
fxpt_16 Conv[],
fxpt_16 FullConv[]);
#endif

#if 0  /* function now implemented in another file */
STATIC void CalcCorEngAdapt(
fxpt_16 y2[MAX_A_LEN],
fxpt_16 residual[RES_LEN],
int     ex_len,
fxpt_32 *cor,
fxpt_32 *eng);
#endif

STATIC void CompGainErrAdapt(
fxpt_32 correlation,
fxpt_32 energy,
fxpt_32 *gain,
fxpt_32 *match);

/**************************************************************************
*
* ROUTINE
*               CalcACBParms
*
* FUNCTION
*               Find ACB gain and error (match score)
*
* SYNOPSIS
*               CalcACBParms(Exc, Exc_len, pc_imp, first, delay, trunc_len,
*                       residual, gain, match)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       Exc             fxpt_16 i       excitation vector
*       Exc_len         int     i       size of ex
*       pc_imp          fxpt_16 i       impulse response of PCs
*       first           int     i       first call flag
*       delay           int     i       pitch lag
*       trunc_len       int     i       length to truncate impulse response
*       residual        fxpt_16 i       residual from LPC analysis
*       match           fxpt_32 o       negative partial squared error
*       gain            fxpt_32 o       optimal gain for excitation vector
*
*==========================================================================
*
* DESCRIPTION
*
* (The calculations below may be valid for version 3.2, but may not be
*  correct for version 3.3).
*
*       For each delay:
*          a.  Filter excitation vector through truncated
*              impulse response of perceptual weighting filter
*              (LPC filter with bandwidth broadening).
*          b.  Correlate filtered result with residual
*          c.  Compute first order pitch filter coefficient (gain)
*              and error (match) for each lag.
*
*       Note:  Proper selection of the convolution length depends on
*              the perceptual weighting filter's expansion factor (gamma)
*              which controls the damping of the impulse response.
*
*               This is one of CELP's most computationally intensive
*               routines.  Neglecting overhead, the approximate number of
*               DSP instructions (add, multiply, multiply accumulate, or
*               compare) per second (IPS) is:
*
*               C:  convolution (recursive truncated end-point correction)
*               C': convolution (recursive truncated end-point correction)
*               R = E:  full correlation & energy
*               R'= E': delta correlation & energy
*               G:      gain quantization
*               G':     delta gain quantization
*
*               IPS = 2.34 M (for integer delays)
*
*       ACB search complexity for integer delays:
*       implicit undefined(a-z)
*
*       DSP chip instructions/operation:
*       integer MUL, ADD, SUB, MAC, MAD, CMP
*       parameter (MUL=1)       !multiply
*       parameter (ADD=1)       !add
*       parameter (SUB=1)       !subtract
*       parameter (MAC=1)       !multiply & accumulate
*       parameter (MAD=2)       !multiply & add
*       parameter (CMP=1)       !compare
*
*       CELP algorithm parameters:
*       integer L, len, K, Kp, Np, dmin
*       real F
*       parameter (L=60)        !subframe length
*       parameter (len=30)      !length to truncate calculations (<= L)
*       parameter (K=2)         !number of full search subframes
*       parameter (Kp=2)        !number of delta search subframes
*       parameter (F=30.e-3)    !time (seconds)/frame
*       parameter (Np=32)       !number of delta subframe delays
*       parameter (dmin=20)     !minimum delay
*
*       integer j
*       parameter (j=4)
*       integer N(j), i
*       real C, R, E, G, Cp, Rp, Ep, Gp, IPS
*       data N/32, 64, 128, 256/
*       print 1
*1      format(10x,'N',10x,'C',13x,'R',12x,'E',15x,'G',13x,'MIPS')
*       do i = 1, j
*          C = (len*(len+1)/2 + len*len)*MAC + (N(i)-1)*len*MAD
C     &      + min(L-dmin,N(i))*(L-dmin)*ADD + (L/2-dmin)*(L-2*dmin)*ADD
C          Cp= (len*(len+1)/2 + len*len)*MAC + (Np-1)*len*MAD
C     &      + min(L-dmin,Np)*(L-dmin)*ADD + (L/2-dmin)*(L-2*dmin)*ADD
C          R = N(i)*L*MAC
C          Rp= Np*L*MAC
C          E = R
C          Ep= Rp
C          G = N(i)*(1*CMP+1*MUL + 1*MUL)
C          Gp= Np*(1*CMP+1*MUL + 1*MUL)
C          IPS = ((C+R+E+G)*K + (Cp+Rp+Ep+Gp)*Kp)/F
C          print *,N(i),C*K/1.e6/F,R*K/1.e6/F,E*K/1.e6/F,G*K/1.e6/F,IPS/1.e6
C          print *,Np,Cp*Kp/1.e6/F,Rp*Kp/1.e6/F,Ep*Kp/1.e6/F,Gp*Kp/1.e6/F
C       end do
C       end
C
C  N          C             R            E               G             MIPS
C  32  0.3136667      0.1280000      0.1280000      6.4000003E-03   1.152133
C  32  0.3136667      0.1280000      0.1280000      6.4000003E-03
C  64  0.4630000      0.2560000      0.2560000      1.2800001E-02   1.563867
C  32  0.3136667      0.1280000      0.1280000      6.4000003E-03
C 128  0.7190000      0.5120000      0.5120000      2.5600001E-02   2.344667
C  32  0.3136667      0.1280000      0.1280000      6.4000003E-03
C 256   1.231000       1.024000       1.024000      5.1200002E-02   3.906267
C  32  0.3136667      0.1280000      0.1280000      6.4000003E-03
C
**************************************************************************
*
*       Compute gain and error:
*         NOTE: Actual MSPE = e0.e0 - gain(2*correlation-gain*energy)
*               since e0.e0 is independent of the code word,
*               minimizing MSPE is equivalent to maximizing:
*                    match = gain(2*correlation-gain*energy)   (1)
*               If unquantized gain is used, this simplifies:
*                    match = correlation*gain
*
*         NOTE: Inferior results were obtained when quantized
*               gain was used in equation (1)???
*
*         NOTE: When the delay is less than the frame length, "match"
*               is only an approximation to the actual error.
*
*       Independent (open-loop) quantization of gain and match (index):

***************************************************************************/

void CalcACBParms(
fxpt_16 Exc[],                          /* 15.0 format */
fxpt_16 pc_imp[],                       /* 3.12 format */
int     first,
int     delay,
fxpt_16 residual[RES_LEN],              /* 15.0 format */
fxpt_32 *match,                         /* 30.1 format */
fxpt_32 *gain)                          /* 17.14 format */
{
        static fxpt_32  Conv1[MAX_A_LEN];       /* 18.13 format */	/* used to be 15.0 format */
        //fxpt_16               FullConv[MAX_A_LEN];
        fxpt_32         correlation, energy;


FXPT_PUSHSTATE("CalcACBParms", -1.0, -1.0);
        if (first) {
                /* Calculate and save convolution of truncated impulse
                 * response for first delay
                 */
                CalcAdaptConv(pc_imp, Exc, Conv1);
        }
        else {
                /* End correct the convulution sum on subsequent delays */
                EndCorrectAdapt(Conv1, Exc, pc_imp);
        }

        if (delay < SF_LEN) {  /* this conditional is a "DaveH" optimization */
          /* For lags shorter than frame size, replicate the short adaptive
           * codeword to the full codeword length
           */
          ReplicateShortToFullCalCorEngAdapt(delay, Conv1, residual, &correlation, &energy);

          /* Calculate Correlation and Energy */
          //CalcCorEngAdapt(FullConv, residual, &correlation, &energy);
        }
        else {
          CalcCorEngAdapt(Conv1, residual, &correlation, &energy);
        }

LOGFIXED(1,correlation);
LOGFIXED(1,energy);

        /* Compute gain and match score */
        CompGainErrAdapt(correlation, energy, gain, match);

FXPT_POPSTATE();
}

/**************************************************************************
*
* ROUTINE
*               CalcAdaptConv
*
* FUNCTION
*               Calculate and save convolution of truncated impulse
*               response.
* SYNOPSIS
*               CalcAdaptConv(pc_imp, len_trunc, Exc, Exc_len, conv);
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       pc_imp          fxpt_16  i      Impulse response of PCs
*       len_trunc       int      i      Length to truncate impulse response
*       Exc             fxpt_16  i      Excitation vector
*       Exc_len         int      i      Lenght of excitation vector
*       conv            fxpt_16  o      Truncated convolved impulse response
*
**************************************************************************
           Calculate and save convolution of truncated (to len)
           impulse response for first lag of t (=mmin) samples:

                min(i, len)
           y     =  SUM  h * ex       , where i = 1, ..., L points
            i, t    j=1   j    i-j+1

                      h |1 2...len x x|
           ex |L . . . 2 1|             = y(1)
             ex |L . . . 2 1|           = y(2)
                           :              :
                     ex |L  . . .  2 1| = y(L)
**************************************************************************/

#if 0  /* now implemented in its own file */
void CalcAdaptConv(
fxpt_16 pc_imp[],                       /* 2.13 format */
int     len_trunc,
fxpt_16 Exc[],                          /* 15.0 format */
int     Exc_len,
fxpt_16 conv[])                         /* 15.0 format */
{
        int     i, j;

FXPT_PUSHSTATE("CalcAdaptConv", -1.0, -1.0);
        /* Calculate convolution */
        for (i=0; i<Exc_len; i++) {
                conv[i] = 0;
                for(j=0; j<=min(i, len_trunc); j++) {
                        /*conv[i] += pc_imp[j] * Exc[i-j];*/
                        conv[i] = fxpt_add16(conv[i],
                            fxpt_mult16_round(pc_imp[j], Exc[i-j], 13));
                }
        }
FXPT_POPSTATE();
}
#endif


/**************************************************************************
*
* ROUTINE
*               EndCorrectAdapt
*
* FUNCTION
*               End correct the convulution sum
* SYNOPSIS
*               EndCorrectAdapt(conv, Exc, Exc_len, pc_imp, len_trunc)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       conv            fxpt_16 i/o     Truncated convolved impulse response
*       Exc             fxpt_16  i      Excitation vector
*       Exc_len         int      i      Length of excitation vector
*       pc_imp          fxpt_16  i      Impulse response of PCs
*       len_trunc       int      i      Length of truncated impulse response
*
**************************************************************************
*          End correct the convolution sum on subsequent delays:
*
*          y     =  0
*           0, t
*          y     =  y        + ex  * h   where i = 1, ..., L points
*           i, m     i-1, m-1   -m    i  and   m = t+1, ..., tmax lags
*
**************************************************************************/

#if 0  /* routine now has a dedicated file */
void EndCorrectAdapt(
fxpt_16 conv[],                         /* 15.0 format */
fxpt_16 Exc[],                          /* 15.0 format */
int     Exc_len,
fxpt_16 pc_imp[],                       /* 2.13 format */
int     len_trunc)
{
        int i;

FXPT_PUSHSTATE("EndCorrectAdapt", -1.0, -1.0);
        for (i=len_trunc-1; i>=1; i--) {
                /*conv[i-1] += Exc[0] * pc_imp[i];*/
                conv[i-1] = fxpt_add16(conv[i-1],
                    fxpt_mult16_round(Exc[0], pc_imp[i], 13));
        }

        for(i=Exc_len;i>=1;i--) {
                conv[i] = conv[i-1];
        }

        /*conv[0] = Exc[0] * pc_imp[0];*/
        conv[0] = fxpt_mult16_round(Exc[0], pc_imp[0], 13);
FXPT_POPSTATE();
}
#endif


/**************************************************************************
*                                                                         *
* ROUTINE
*               ReplicateShortToFull
* FUNCTION
*               Replicate the short adaptive codeword to the full codeword
*               length
* SYNOPSIS
*               ReplicateShortToFull(Exc_len, delay, Conv, FullConv)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       Exc_len         int      i      Length of excitation vector
*       delay           int      i      Pitch lag
*       Conv            fxpt_16  i      Truncated convolved impulse response
*       FullConv        fxpt_16  o      Replicated convolved impulse response
*
**************************************************************************/

#if 0  /* Routine no longer called */
void ReplicateShortToFull(
int     delay,
fxpt_16 Conv[],                         /* 15.0 format */
fxpt_16 FullConv[])
{
        int     i;

FXPT_PUSHSTATE("ReplicateShortToFull", -1.0, -1.0);
        MoveArray16 (FullConv, Conv, SF_LEN);
        // for (i=0; i<SF_LEN; i++) {
        //   FullConv[i] = Conv[i];
        // }

        if (delay < SF_LEN) {
                /*  Add in 2nd convolution */
                for (i=delay; i<SF_LEN; i++) {
                        /*FullConv[i] += Conv[i-delay];*/
                        FullConv[i] = fxpt_add16(FullConv[i], Conv[i-delay]);
                }

                if (delay < (SF_LEN / 2)) {
                        /* Add in 3rd convolution */
                        for (i=2*delay; i< SF_LEN; i++) {
                                /*FullConv[i] += Conv[i-2*delay];*/
                                FullConv[i] = fxpt_add16(FullConv[i],
                                    Conv[i-2*delay]);
                        }
                }
        }
FXPT_POPSTATE();
}
#endif

/**************************************************************************
*                                                                         *
* ROUTINE
*               CalcCorEngAdapt
* FUNCTION
*               Calculate Correlation and Energy
* SYNOPSIS
*               CalcCorEngAdapt(AnaSignal, MatchSignal, length,
*                       correlation, energy)
*
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       AnaSignal       fxpt_16  i      Signal to analyze
*       MatchSignal     fxpt_16  i      Signal to match
*       length          int      i      Length of signals
*       correlation     fxpt_32  o      Correlation between to signals
*       energy          fxpt_32  o      Energy in signal to analyze
*
*
**************************************************************************/

#if 0

void CalcCorEngAdapt(
fxpt_16 AnaSignal[],                    /* 15.0 format */
fxpt_16 MatchSignal[],                  /* 15.0 format */
int     length,
fxpt_32 *correlation,                   /* 30.1 format */
fxpt_32 *energy)                        /* 30.1 format */
{
        int     i;

FXPT_PUSHSTATE("CalcCorEngAdapt", -1.0, -1.0);
        *correlation = 0;               /* need 27 bits for this */
        *energy = 0;                    /* need 27 bits for this */

        for(i=0; i<length; i++) {
                /* 32 bits may be inadequate to hold the non-normalized
                 * correlation and energy values.  Since we are interested
                 * in the relative values anyway, we scale these values
                 * by shifting them to the right 4 times (* 1/16).
                 */
                /**correlation += AnaSignal[i] * MatchSignal[i];*/
                *correlation = fxpt_add32(*correlation, fxpt_shr32_fast(
                    fxpt_mult32(AnaSignal[i], MatchSignal[i]), 4));
                /**energy += AnaSignal[i] * AnaSignal[i];*/
                *energy = fxpt_add32(*energy, fxpt_shr32_fast(
                    fxpt_mult32(AnaSignal[i], AnaSignal[i]), 4));
        }
FXPT_POPSTATE();
}

#endif


/**************************************************************************
*                                                                         *
* ROUTINE
*               CompGainErrAdapt
* FUNCTION
*               Compute gain and error
* SYNOPSIS
*               CompGainErrAdapt(correlation, energy, gain, match)
*
*
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       correlation     fxpt_32  i      Correlation
*       energy          fxpt_32  i      Energy
*       gain            fxpt_32  o      Adaptive Codebook gain
*       match           fxpt_32  o      Negative partial squared error
*
*
**************************************************************************/

void CompGainErrAdapt(
fxpt_32 correlation,            /* 30.1 format */
fxpt_32 energy,                 /* 30.1 format */
fxpt_32 *gain,                  /* 17.14 format */
fxpt_32 *match)                 /* 30.1 format */
{
        if (energy <= 0)
                energy = 1;

FXPT_PUSHSTATE("CompGainErrAdapt", -1.0, -1.0);
        /**gain = correlation / energy;*/
        *gain = fxpt_div32(correlation, energy, 14);

        /**match = correlation * *gain;*/
        *match = fxpt_mult64_fix(correlation, *gain, 14);
FXPT_POPSTATE();
}
